#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include <math.h> 

int_fast32_t count;
int_fast32_t count_t;
int_fast32_t count_t1=0;

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"control");
    ros::NodeHandle nh;
    // 3.创建发布者对象
    ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",1000);
    // 4.循环发布运动控制消息
    //4-1.组织消息
    geometry_msgs::Twist msg;




    msg.linear.x = 1.5;
    msg.linear.y = 0.0;
    msg.linear.z = 0.0;

    msg.angular.x = 0.0;
    msg.angular.y = 0.0;
    msg.angular.z = 0.6;

    //4-2.设置发送频率
    ros::Rate r(10);
    //4-3.循环发送
    while (ros::ok())
    {
        // count_t1++;
        // if(count_t1>1000000){
        //     msg.angular.z = 0.0;//0.7854
        // }else{
        // }

        // count++;
        // count_t=count/1600000;
        // if(count_t%2==0 ){
        // msg.linear.x = 2.0;
        // }else {
        // msg.linear.x = -2.0;
        // }
       

        pub.publish(msg);

        ros::spinOnce();
    }
    return 0;
}
